import time

from machine import Pin, I2C
from oled_0_96 import OledStr
from mpu6050_driver import Mpu6050
from led import set_led_on, set_led_off
from pid import StandPID

i2c1 = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
i2c2 = I2C(1, scl=Pin(18), sda=Pin(19))

if __name__ == "__main__":
    oled = OledStr(i2c2)
    mpu6050 = Mpu6050(i2c1)
    stand_pid = StandPID(19.5, 5.4)
    set_led_on()
    while True:
        oled.clear_oled()
        gyro_x, gyro_y, gyro_z = mpu6050.read_gyro_data()
        pitch, roll = mpu6050.accel_to_angle()
        speed = stand_pid.update_speed(pitch, gyro_y, target_roll=0)
        gyro_y_str = "GyroY:{:.2f}".format(gyro_y)
        pitch_str = "Pitch:{:.2f}".format(pitch)
        speed_str = f"Speed:{speed}"
        oled.fill_text(gyro_y_str, 0, 0)
        oled.fill_text(pitch_str, 0, 16)
        oled.fill_text(speed_str, 0, 32)
        oled.show_text()


